#ifndef LASER_SCAN_MATCHER_LASER_SCAN_MAPPING_H
#define LASER_SCAN_MATCHER_LASER_SCAN_MAPPING_H

#include <rclcpp/rclcpp.hpp> 
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>
#include <pcl/filters/radius_outlier_removal.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/msg/odometry.hpp>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>

#include <laser_geometry/laser_geometry.h>
#include <laser_scan_mapping/keyframe.h>
#include <laser_scan_mapping/keyframe_updater.h>
#include <laser_scan_mapping/ros_utils.h>
#include <laser_scan_mapping/map_cloud_generator.h>
#include <laser_scan_mapping/graph_slam.h>
#include <laser_scan_mapping/ros_time_hash.h>
#include <laser_scan_mapping/information_matrix_calculator.h>
#include <laser_scan_mapping/loop_detector.hpp>
#include <laser_scan_graph_slam/SaveMap.h>

#include <tf2_ros/transform_listener.h>
#include "tf2_ros/static_transform_broadcaster.h"
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_sensor_msgs/msg/tf2_sensor_msgs.hpp>

#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include <g2o/edge_se3_plane.hpp>
#include <g2o/edge_se3_priorxy.hpp>
#include <g2o/edge_se3_priorxyz.hpp>
#include <g2o/edge_se3_priorvec.hpp>
#include <g2o/edge_se3_priorquat.hpp>

#include <opencv2/opencv.hpp>

#include <pcl/filters/crop_box.h>


namespace laser_scan_graph_slam{

static const unsigned char FREE_SPACE = 0;
static const unsigned char LETHAL_OBSTACLE = 100;
static const unsigned char NO_INFORMATION = -1;

struct Frame {
  pcl::PointCloud<pcl::PointXYZI> cloud;
  Eigen::Isometry3d odom;
}; 

class LaserScanMapping
{
  public:
    LaserScanMapping(h, h_private);
    ~LaserScanMapping();

  private:
    typedef pcl::PointXYZI           PointT;
    typedef pcl::PointCloud<PointT> PointCloudT;
    typedef message_filters::sync_policies::ApproximateTime<nav_msgs::msg::Odometry, sensor_msgs::msg::LaserScan> ApproxSyncPolicy;

    // **** ros
    h_;
    h_private_;
    ros::WallTimer optimization_timer;
    ros::WallTimer map_publish_timer;

    std::shared_ptr<tf2_ros::TransformListener> tfl_;
    std::shared_ptr<tf2_ros::Buffer> tf_;

    rclcpp::Publisher scan_cloud_pub_, base_scan_cloud_pub_;
    rclcpp::Publisher map_points_pub_;
    rclcpp::Publisher markers_pub_;
    rclcpp::Publisher map_grid_pub_;
    rclcpp::Publisher debug_scan_pub_;
    rclcpp::Subscription scan_subscriber_;
    


    std::unique_ptr<message_filters::Subscriber<nav_msgs::msg::Odometry>> odom_sub;
    std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> scan_sub;
    std::unique_ptr<message_filters::Synchronizer<ApproxSyncPolicy>> sync;

    ros::ServiceServer save_map_service_server;

    laser_geometry::LaserProjection projector_;
    std::unique_ptr<KeyframeUpdater> keyframe_updater;

    // tf::TransformListener    tf_listener_;
    // tf::TransformBroadcaster tf_broadcaster_;
    tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;

    // tf::Transform base_to_laser_; // static, cached
    // tf::Transform laser_to_base_; // static, cached, calculated from base_to_laser_

    // rclcpp::Publisher  pose_publisher_;

    // **** parameters
    std::string map_frame_id;
    std::string odom_frame_id;
    std::string robot_frame_id;

    std::mutex trans_odom2map_mutex;
    Eigen::Matrix4f trans_odom2map;

    std::deque<Frame> tmp_frame_queue;

    // keyframe queue
    std::string base_frame_id;
    std::mutex keyframe_queue_mutex;
    std::deque<KeyFrame::Ptr> keyframe_queue;

    bool use_distance_filter;
    double distance_near_thresh;
    double distance_far_thresh;
    double distance_rear_thresh;


    // **** state variables

    boost::mutex mutex_;
    
    std::atomic_bool graph_updated;
    double map_cloud_resolution;
    std::mutex keyframes_snapshot_mutex;
    std::vector<KeyFrameSnapshot::Ptr> keyframes_snapshot;
    std::unique_ptr<MapCloudGenerator> map_cloud_generator;
    std::vector<KeyFrame::Ptr> keyframes;
    std::unordered_map<rclcpp::Time, KeyFrame::Ptr, RosTimeHash> keyframe_hash;

    std::mutex main_thread_mutex;
    
    int max_keyframes_per_update;
    std::deque<KeyFrame::Ptr> new_keyframes;

    KeyFrame::Ptr pre_keyframe;

    g2o::VertexSE3* anchor_node;
    g2o::EdgeSE3* anchor_edge;

    std::unique_ptr<GraphSLAM> graph_slam;

  
    bool init_sensor_to_base = false;
    Eigen::Matrix4f base_to_senser_mat;
    geometry_msgs::msg::TransformStamped base_to_senser_trans;

    std::unique_ptr<InformationMatrixCalculator> inf_calclator;
    std::unique_ptr<LoopDetector> loop_detector;

    // **** methods ****
    void initParams();
    void scanCallback (const nav_msgs::msg::OdometryConstPtr& odom_msg, const sensor_msgs::msg::LaserScan& scan_msg);
    void map_points_publish_timer_callback(const ros::WallTimerEvent& event);
    void optimization_timer_callback(const ros::WallTimerEvent& event);

    bool save_map_service(laser_scan_graph_slam::SaveMapRequest& req, laser_scan_graph_slam::SaveMapResponse& res);

    bool flush_keyframe_queue();
    visualization_msgs::msg::MarkerArray create_marker_array(const rclcpp::Time& stamp) const;
    void pointcloud_to_occupancy_grid( const std::vector<KeyFrameSnapshot::Ptr>& keyframes, const pcl::PointCloud<PointT>& cloud);
    pcl::PointCloud<PointT> distance_filter(const pcl::PointCloud<PointT>& cloud) const;
};

} // namespace laser_scan_graph_slam

#endif // LASER_SCAN_MATCHER_LASER_SCAN_MAPPING_H

